#ifndef TRACK_FILE_HELPER_H_
#define TRACK_FILE_HELPER_H_

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/json.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <fstream>
#include <iostream>
#include <sstream>

#include "common/map_info.h"
#include "track/settings.h"

namespace autodrive {
namespace perception {
inline bool IsFileExist(const std::string& path) noexcept {
  std::ifstream stream{path};
  bool isExit = stream.good();
  stream.close();
  return isExit;
}

inline bool CreateDirectory(const std::string& path,
                            std::string& err) noexcept {
  boost::system::error_code errorCode;
  auto const& bRet{boost::filesystem::create_directories(path, errorCode)};
  if (!bRet) {
    err = "create director: " + path + " fail, " + errorCode.message();
  }
  return bRet;
}

inline void ReadTrackParameter(const std::string& path,
                               MapTrackParameter& para) {
  boost::property_tree::ptree root;
  boost::property_tree::read_json(path, root);
  para.turn_score_threshold = root.get<float>("turn_score_threshold");
  para.cross_score_threshold = root.get<float>("cross_score_threshold");
  para.nostop_score_threshold = root.get<float>("nostop_score_threshold");
  para.DBSCAN_eps_turn = root.get<float>("DBSCAN_eps_turn");
  para.DBSCAN_min_num_turn = root.get<int>("DBSCAN_min_num_turn");
  para.DBSCAN_eps_crosswalk = root.get<float>("DBSCAN_eps_crosswalk");
  para.DBSCAN_min_num_crosswalk = root.get<int>("DBSCAN_min_num_crosswalk");
  para.DBSCAN_eps_nostop = root.get<float>("DBSCAN_eps_nostop");
  para.DBSCAN_min_num_nostop = root.get<int>("DBSCAN_min_num_nostop");
  para.s_match_2DIOU_minimum = root.get<float>("s_match_2DIOU_minimum");
  para.s_match_distance_maximum = root.get<float>("s_match_distance_maximum");
  para.collect_track_consecutive_invisible_maximum =
      root.get<int>("collect_track_consecutive_invisible_maximum");
  para.collect_track_age_minimum = root.get<int>("collect_track_age_minimum");

  para.position_filter_alpha = root.get<float>("position_filter_alpha");
  para.length_filter_alpha = root.get<float>("length_filter_alpha");
  para.width_filter_alpha = root.get<float>("width_filter_alpha");
  para.yaw_filter_alpha = root.get<float>("yaw_filter_alpha");
}

inline void WriteTrackParameter(const std::string& path,
                                MapTrackParameter& para) {
  // boost::property_tree::ptree root;
  // root.put("turn_score_threshold", para.turn_score_threshold);
  // root.put("cross_score_threshold", para.cross_score_threshold);
  // root.put("nostop_score_threshold", para.nostop_score_threshold);
  // root.put("DBSCAN_eps", para.DBSCAN_eps);
  // root.put("DBSCAN_min_num", para.DBSCAN_min_num);
  // write_json(path, root);

  std::fstream os;
  os.open(path, std::ios_base::out);
  if (!os.is_open()) {
    std::cout << "Open Path " << path << "  Error!!!!!!!" << std::endl;
    return;
  }
  // 设置json内容
  boost::json::object rootObject;  // boost 1.79中 的json对象
  rootObject["turn_score_threshold"] = para.turn_score_threshold;
  rootObject["cross_score_threshold"] = para.cross_score_threshold;  // 赋值操作
  rootObject["nostop_score_threshold"] =
      para.nostop_score_threshold;                               // 赋值操作
  rootObject["DBSCAN_eps_turn"] = para.DBSCAN_eps_turn;          // 赋值操作
  rootObject["DBSCAN_min_num_turn"] = para.DBSCAN_min_num_turn;  // 赋值操作
  rootObject["DBSCAN_eps_crosswalk"] = para.DBSCAN_eps_crosswalk;  // 赋值操作
  rootObject["DBSCAN_min_num_crosswalk"] =
      para.DBSCAN_min_num_crosswalk;                         // 赋值操作
  rootObject["DBSCAN_eps_nostop"] = para.DBSCAN_eps_nostop;  // 赋值操作
  rootObject["DBSCAN_min_num_nostop"] = para.DBSCAN_min_num_nostop;  // 赋值操作
  rootObject["s_match_2DIOU_minimum"] = para.s_match_2DIOU_minimum;  // 赋值操作
  rootObject["s_match_distance_maximum"] =
      para.s_match_distance_maximum;  // 赋值操作
  rootObject["collect_track_consecutive_invisible_maximum"] =
      para.collect_track_consecutive_invisible_maximum;  // 赋值操作
  rootObject["collect_track_age_minimum"] =
      para.collect_track_age_minimum;  // 赋值操作
  rootObject["position_filter_alpha"] = para.position_filter_alpha;  // 赋值操作
  rootObject["length_filter_alpha"] = para.length_filter_alpha;  // 赋值操作
  rootObject["width_filter_alpha"] = para.width_filter_alpha;    // 赋值操作
  rootObject["yaw_filter_alpha"] = para.yaw_filter_alpha;        // 赋值操作
  os << rootObject;                                              // 写到文件
  os.close();                                                    // 关闭文件
}

/// @brief extract map data
/// @param map map to read
/// @param
inline void extract_map_roadmarking_data(
    autodrive::perception::Map const& map,
    std::vector<Eigen::Vector2f>& turn_points,
    std::vector<Eigen::Vector2f>& crosswalk_points,
    std::vector<Eigen::Vector2f>& nostop_points, float turn_score_threshold,
    float crosswalk_score_threshold, float nostop_score_threshold) noexcept {
  int count = map.road_marking.size();
  // std::cout << " extract_map_roadmarking_data count = " << count <<
  // std::endl;
  turn_points.clear();
  crosswalk_points.clear();
  nostop_points.clear();
  for (int i = 0; i < count; ++i) {
    const autodrive::perception::RoadMarking& marking = map.road_marking.at(i);
    const autodrive::perception::RoadMarkingCategory& category =
        marking.category.at(0);
    if (category == autodrive::perception::RoadMarkingCategory::STRAIGHT ||
        category == autodrive::perception::RoadMarkingCategory::LEFT ||
        category == autodrive::perception::RoadMarkingCategory::RIGHT ||
        category == autodrive::perception::RoadMarkingCategory::U_TURN ||
        category ==
            autodrive::perception::RoadMarkingCategory::CONFLUENCE_LEFT ||
        category ==
            autodrive::perception::RoadMarkingCategory::CONFLUENCE_RIGHT) {
      int points_count = marking.points.size();
      for (int j = 0; j < points_count; j++) {
        // std::cout << "extract turn" << std::endl;
        const autodrive::perception::MapElementPoint& point =
            marking.points.at(j);
        // std::cout << "turn : " << point.x << " " << point.y << " " << point.z
        //           << " " << point.score << std::endl;
        if (point.score > turn_score_threshold) {
          turn_points.push_back(Eigen::Vector2f{point.x, point.y});
        }
      }
    } else if (category ==
               autodrive::perception::RoadMarkingCategory::CROSSWALK) {
      std::cout << "extract crosswalk" << std::endl;
      int points_count = marking.points.size();
      for (int j = 0; j < points_count; j++) {
        const autodrive::perception::MapElementPoint& point =
            marking.points.at(j);
        // std::cout << "crosswalk : " << point.x << " " << point.y << " "
        //           << point.z << " " << point.score << std::endl;
        if (point.score > crosswalk_score_threshold) {
          crosswalk_points.push_back(Eigen::Vector2f{point.x, point.y});
        }
      }

    } else if (category ==
               autodrive::perception::RoadMarkingCategory::NO_STOP) {
      // std::cout << "extract no_stop" << std::endl;
      int points_count = marking.points.size();
      for (int j = 0; j < points_count; j++) {
        const autodrive::perception::MapElementPoint& point =
            marking.points.at(j);
        // std::cout << "nostop : " << point.x << " " << point.y << " " <<
        // point.z
        //           << " " << point.score << std::endl;
        if (point.score > nostop_score_threshold) {
          nostop_points.push_back(Eigen::Vector2f{point.x, point.y});
        }
      }
    }
  }
}

}  // namespace perception
}  // namespace autodrive

#endif